//
// Created by buenos on 2021/3/6.
//
#include "Tracker.h"

void Tracker::readImage(const cv::Mat &img_, const cv::Mat &depth_, double curTime_) {
    // TODO: clahe
    curIamge = img_;
    cv::goodFeaturesToTrack(curIamge, curPts, 100, 0.1, 3.0, cv::Mat());

//    cv::drawKeypoints()

}

void Tracker::genOutputImage(const cv_bridge::CvImageConstPtr &imagePtr) {
    // draw points
    {
        cv::Mat img = imagePtr->image;
        for(cv::Point2f p: curPts){
            cv::circle(img, p, 2, cv::Scalar(255, 0, 0), 2);
        }
        outMsg.image = img;
    }

    outMsg.header = imagePtr->header;
    outMsg.encoding = sensor_msgs::image_encodings::MONO8;

}